#include <crate_vision/main.h>
#include "crate_vision/CrateVisionNode.h"
#include "crate_vision/Detect.h"
#include <iostream>
#include <stdio.h>
#include <FiducialDetector.h>
#include <pcrctransformation/pcrctransformer.hpp>
#include <pcrctransformation/point2f.hpp>

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/core/core.hpp>
#include <unicap_cv_bridge.hpp>
#include <CameraCalibration/RectifyImage.h>

using namespace cv;
using namespace unicap_cv_bridge;
using namespace std;
using namespace pcrctransformation;

/* 
 * This function is used to scan the crate and detect which boxes are empty. NOTE: AT THIS THE POINT THIS FUNCTION DOES NOT RETURN ANYTHING USEFULL!!
 *
 * First the crate gets cut out and split into pieces. Then the pieces will be checked for color and hue to determine if they are empty
 */
bool Detect::scan_crate(){
		Mat frame(cam->get_img_height(), cam->get_img_width(), cam->get_img_format()), final_frame;
    	int box_check[getVisionData()->crateData.row_nr * getVisionData()->crateData.col_nr][4];
    	
    	cam->get_frame(&frame);
	
		rectifier->rectify(frame, frameRectified);
		
		medianBlur(frameRectified, frameRectified, getVisionData()->crateData.median);
		
	//cout << "detect::scancrate  pixel: " << getVisionData()->crateData.middle_x << ", " << getVisionData()->crateData.middle_y << "  real: " << getVisionData()->robotCrateData.middle_x << ", " << getVisionData()->robotCrateData.middle_y << endl;
		
		final_crate = get_roi(frameRectified, Point2f(getVisionData()->crateData.middle_x, getVisionData()->crateData.middle_y), getVisionData()->crateData.degree, getVisionData()->crateData.width, getVisionData()->crateData.height);	//haal plaatje van de crate op
		
		
		split_crate(frameRectified, Point2f(getVisionData()->robotCrateData.middle_x, getVisionData()->robotCrateData.middle_y), getVisionData()->robotCrateData.degree);
		/*
		draw_box(frameRectified, 
					getVisionData()->crateData.middle_x - (getVisionData()->crateData.width/2),
					getVisionData()->crateData.middle_y - (getVisionData()->crateData.height/2), 
					getVisionData()->crateData.middle_x + (getVisionData()->crateData.width/2), 
					getVisionData()->crateData.middle_y + (getVisionData()->crateData.height/2), 
					Scalar(0, 255, 0));*/
					
		
		imshow("final_crate", final_crate); 	
		imshow("median filter", frameRectified); 
		waitKey(30);
	    
		cout << "----------------------------------------------" << endl;
		
		int hue_peak, saturation_peak, value_peak, peak_value;
		for (int l = 0; l < getVisionData()->crateData.row_nr*getVisionData()->crateData.col_nr; l++) {
		/*
		imshow("box", getVisionData()->contentData.box[l]);
		waitKey(0);
		*/
			hsv_hist(getVisionData()->contentData.box[l], &hue_peak, 0, &peak_value);
			hsv_hist(getVisionData()->contentData.box[l], &saturation_peak, 1, &peak_value);
			hsv_hist(getVisionData()->contentData.box[l], &value_peak, 2, &peak_value);
			
			box_check[l][1] = hue_peak;
			box_check[l][2] = saturation_peak;
			box_check[l][3] = value_peak;	
			
			if (hue_peak >= getVisionData()->contentData.empty_hue_low && hue_peak <= getVisionData()->contentData.empty_hue_high) {
				draw_box(final_crate, 
							(l%getVisionData()->crateData.col_nr) * getVisionData()->crateData.container_width + getVisionData()->crateData.offset, 
							l/getVisionData()->crateData.col_nr * getVisionData()->crateData.container_height + getVisionData()->crateData.offset, 
							(l%getVisionData()->crateData.col_nr * getVisionData()->crateData.container_width) + getVisionData()->crateData.container_width - getVisionData()->crateData.offset, 
							l/getVisionData()->crateData.col_nr * getVisionData()->crateData.container_height + getVisionData()->crateData.container_height - getVisionData()->crateData.offset, 
							Scalar(0, 0, 255));
				
				box_check[l][0] = 0;
				cout << "EMPTY: ";
			} else {
					box_check[l][0] = getVisionData()->contentData.diff_colors+1;
					for(int i = 0; i < getVisionData()->contentData.diff_colors; i++){
						if(getVisionData()->contentData.ball_hue_low[i] > getVisionData()->contentData.ball_hue_high[i]){
							if (hue_peak >= getVisionData()->contentData.ball_hue_low[i] || hue_peak <= getVisionData()->contentData.ball_hue_high[i]){
								box_check[l][0] = i+1;
								break;
							}
							
						} else {
							if (hue_peak >= getVisionData()->contentData.ball_hue_low[i] && hue_peak <= getVisionData()->contentData.ball_hue_high[i] ){
								box_check[l][0] = i+1;
								break;
							}
						}
					}
					
			}
			cout << "pos: " << l << " hue: " << hue_peak << endl;
		}
	

		Mat hsvToRgb(getVisionData()->crateData.row_nr,getVisionData()->crateData.col_nr,cam->get_img_format()), temp;
		int red, green, blue;
		
		for(int x = 0; x < getVisionData()->crateData.col_nr; x++){
			for(int y = 0; y < getVisionData()->crateData.row_nr; y++){
				cout << x << ", " <<  y << endl;
				int hue = box_check[x+y*getVisionData()->crateData.col_nr][1];
				int saturation = box_check[x+y*getVisionData()->crateData.col_nr][2];
				int value = box_check[x+y*getVisionData()->crateData.col_nr][3];

				cout << "hue: " << hue << " sat: " << saturation << " val: " << value << endl;
				
				Vec3b pixel(hue, saturation, value);
				hsvToRgb.at<Vec3b>(x,y) = pixel;
			}
		}
		
		cvtColor(hsvToRgb, temp, CV_HSV2BGR);
		
		for(int x = 0; x < getVisionData()->crateData.col_nr; x++){
			for(int y = 0; y < getVisionData()->crateData.row_nr; y++){
				
				Vec3b pixel = temp.at<Vec3b>(x, y);
				red = pixel[2];
				green = pixel[1];
				blue = pixel[0];
				
				cout << x << ", " <<  y << endl;
				cout << "red: " << red << " green: " << green << " blue: " << blue << endl;
				
				getVisionData()->crateData.crate_info_red[x+y*getVisionData()->crateData.col_nr] = red;
				getVisionData()->crateData.crate_info_green[x+y*getVisionData()->crateData.col_nr] = green;
				getVisionData()->crateData.crate_info_blue[x+y*getVisionData()->crateData.col_nr] = blue;
				cout << "box_check crate_info: " << box_check[x+y*getVisionData()->crateData.col_nr][0] << endl;
				getVisionData()->crateData.crate_info[x+y*getVisionData()->crateData.col_nr] = box_check[x+y*getVisionData()->crateData.col_nr][0];
			}
		}
		

		imshow("final_crate", final_crate); 
		waitKey(10);
	
		return true;
}



/*
 * 
 *
 */
Mat Detect::hsv_hist(Mat src, int* max_pos, int channel, int* avg) {
	int bins = 256, max = 0;
	int hist[bins];
	 CvScalar   colors[] = { CV_RGB(255,255,0), CV_RGB(0,255,0),
							  CV_RGB(255,0,0), CV_RGB(0,0,0) };

	Mat temp;
	cvtColor(src, temp, CV_BGR2HSV);
	IplImage iplImg = temp;

	int hist_width = 300, hist_height = 600;
	Mat hist_img(hist_height, hist_width, CV_8UC3);

	  /* Reset histogram */
	  for (int j = 0; j < bins-1; hist[j]=0, j++);

	/* Calc histogram of the image */
	  for (int x = 0; x < iplImg.width; x++){
		  for (int y = 0; y < iplImg.height; y++) {
			  /*
			uchar* ptr = (uchar*)(iplImg.imageData + i * iplImg.widthStep);
			for (int j = 0; j < iplImg.width; j+=iplImg.nChannels)*/
			  Vec3b pixel = temp.at<Vec3b>(x, y);
			  hist[pixel[channel]]++;
			  *(avg)+= pixel[channel];
		  }
	  }

	  /* Get histogram peak */
	  for (int i = 0; i < bins-1; i++) {
		if (hist[i] > max) {
			max = hist[i];
			*max_pos = i;
		}
	  }

	  /* Get scale so the histogram fit the canvas height */
	  double scale = 3;
	  if (max > hist_height) {
		  scale = (double)hist_height/max;
	  }

  /* Draw histogram */
	for (int i = 0; i < bins-1; i++) {
	  Point pt1(i, hist_height - (hist[i] * scale));
	  Point pt2(i, hist_height);
	  line(hist_img, pt1, pt2, colors[channel], 1, 8, 0);
	}

	*(avg) = *(avg)/(iplImg.width*iplImg.height);

	
	return hist_img;
}

/*
 * Function used to check for objects in boxes with imagae correlation. Currently not used
 *
 */
void Detect::matchTemplate_vision() {
	Mat result, result_copy;
	matchTemplate(final_crate, reference, result, CV_TM_SQDIFF);

	result.copyTo(result_copy);

	int temp_x = 0, temp_y = 0;
	int temp_width = getVisionData()->crateData.container_width, temp_height = getVisionData()->crateData.container_height;
	for (int y = 0; y < getVisionData()->crateData.row_nr; y++) {
		temp_x = 0;
		for (int x = 0; x < getVisionData()->crateData.col_nr; x++) {
			temp_width = getVisionData()->crateData.container_width;
			temp_height = getVisionData()->crateData.container_height;

			if (x == 0) temp_width/=2;
			if (x == getVisionData()->crateData.col_nr-1) temp_width/=2;

			if (y == 0) temp_height/=2;
			if (y == getVisionData()->crateData.row_nr-1) temp_height/=2;

			//InputArray mask(create_mask(i, width, height, row_nr, col_nr, result.type()));

			double minVal=0;
			double maxVal=0;
			Point minLoc;
			Point maxLoc;

			Mat temp_cut = result(Rect(temp_x, temp_y, temp_width, temp_height));

			minMaxLoc(temp_cut, &minVal, &maxVal, &minLoc, &maxLoc);

			//waitKey(0);
			if (minVal < 9000000) {
				cout << "----------------------i: " << x + y * getVisionData()->crateData.col_nr << endl;
				cout << minVal << endl;
				cout << minLoc.x << ", " << minLoc.y << endl;
			}

			draw_box(result_copy, 
						temp_x, 
						temp_y,
						temp_x + temp_width, 
						temp_y + temp_height, 
						Scalar(0, 255, 0));

			temp_x+= temp_width;
		}

		temp_y+= temp_height;
	}
	imshow("result", result_copy);
}

void Detect::draw_hist(Mat& dest, const Mat& src, int histSize) {
	rectangle(dest, Point(0, 0), Point(dest.cols, dest.rows), Scalar(0, 0, 0, 0), CV_FILLED, 1, 0);
	// Separate the image in 3 places ( R, G and B )
	vector<Mat> rgb_planes;
	split(src, rgb_planes);

	// Set the ranges ( for R,G,B) )
	float range[] = { 0, 255 } ;
	const float* histRange = { range };

	Mat r_hist, g_hist, b_hist;

	// Compute the histograms:
	calcHist(&rgb_planes[0], 1, 0, Mat(), r_hist, 1, &histSize, &histRange, true, false);
	calcHist(&rgb_planes[1], 1, 0, Mat(), g_hist, 1, &histSize, &histRange, true, false);
	calcHist(&rgb_planes[2], 1, 0, Mat(), b_hist, 1, &histSize, &histRange, true, false);

	// Draw the histograms for R, G and B
	int hist_w = dest.cols, hist_h = dest.rows;
	int bin_w = cvRound((double)(hist_w)/histSize);

	float hist_max = 0;
	for(int i = 0; i < histSize; i++){
		int x = r_hist.at<float>(i);
		if(x > hist_max) { hist_max = x; }
		x = g_hist.at<float>(i);
		if(x > hist_max) { hist_max = x; }
		x = b_hist.at<float>(i);
		if(x > hist_max) { hist_max = x; }
	}

	r_hist *= (dest.rows / (double)hist_max);
	g_hist *= (dest.rows / (double)hist_max);
	b_hist *= (dest.rows / (double)hist_max);

	// Draw for each channel
	int i;
	for(i = 1; i < histSize; i++){
		line(
			dest,
			Point(bin_w * (i - 1), hist_h - cvRound(r_hist.at<float>(i - 1))),
			Point(bin_w * (i), hist_h - cvRound(r_hist.at<float>(i))),
			Scalar(255, 0, 0), 1, 8, 0);
		line(
			dest,
			Point(bin_w * (i - 1), hist_h - cvRound(g_hist.at<float>(i - 1))),
			Point(bin_w * (i), hist_h - cvRound(g_hist.at<float>(i))),
			Scalar(0, 255, 0), 1, 8, 0);
		line(
			dest,
			Point(bin_w * (i - 1), hist_h - cvRound(b_hist.at<float>(i - 1))),
			Point(bin_w * (i), hist_h - cvRound(b_hist.at<float>(i))),
			Scalar(0, 0, 255), 1, 8, 0);
	}

	line(
		dest,
		Point(bin_w * (i - 1), hist_h - cvRound(r_hist.at<float>(i - 1))),
		Point(bin_w * (i), hist_h - cvRound(r_hist.at<float>(i - 1))),
		Scalar(255, 0, 0), 1, 8, 0);
	line(
		dest,
		Point(bin_w * (i - 1), hist_h - cvRound(g_hist.at<float>(i - 1))),
		Point(bin_w * (i), hist_h - cvRound(g_hist.at<float>(i - 1))),
		Scalar(0, 255, 0), 1, 8, 0);
	line(
		dest,
		Point(bin_w * (i - 1), hist_h - cvRound(b_hist.at<float>(i - 1))),
		Point(bin_w * (i), hist_h - cvRound(b_hist.at<float>(i - 1))),
		Scalar(0, 0, 255), 1, 8, 0);
}

void Detect::split_crate(Mat &src, Point2f centre, double angle) {
	point2f position;
	
	double diff_x_vertical, diff_y_vertical, diff_x_horizontal, diff_y_horizontal, x, y;
	diff_x_vertical = sin(angle);
	diff_y_vertical = cos(angle);
	
	diff_x_horizontal = cos(angle);
	diff_y_horizontal = -sin(angle);
	
	double horizontal_distance_to_beat_1 = getVisionData()->robotCrateData.width/2 - getVisionData()->robotCrateData.distance_to_side - 0.5*getVisionData()->robotCrateData.container_width;
	double vertical_distance_to_beat_1 = getVisionData()->robotCrateData.height/2 - getVisionData()->robotCrateData.distance_to_side - 0.5*getVisionData()->robotCrateData.container_height;
	 
	x = centre.x - horizontal_distance_to_beat_1 * diff_x_horizontal - 
						vertical_distance_to_beat_1 * diff_x_vertical;
	
	
	y = centre.y - horizontal_distance_to_beat_1 * diff_y_horizontal -
						vertical_distance_to_beat_1 * diff_y_vertical;
	
	
	for (int i = 0; i < getVisionData()->crateData.row_nr; i++) {
		for (int j = 0; j < getVisionData()->crateData.col_nr; j++) {
			position.x = x + (i * diff_x_horizontal + j * diff_x_vertical)*getVisionData()->robotCrateData.centre_to_centre;
			position.y = y + (i * diff_y_horizontal + j * diff_y_vertical)*getVisionData()->robotCrateData.centre_to_centre;
			
			//point2f pre_perspective_pix = getVisionData()->envData.cordTransformer->to_pc(position);
			
			position = single_perspective_adjustment(position, true);
			
			position = getVisionData()->envData.cordTransformer->to_pc(position);
			
/*			draw_box(src, 
					position.x - 3, 
					position.y - 3, 
					position.x + 3, 
					position.y + 3, 
					Scalar(0, 50*i, 50*j));*/
					
			if (position.x - getVisionData()->crateData.offset < 0) {
				perror("<ERROR> container pixel location: x < 0");
				position.x = getVisionData()->crateData.offset;
			} else if ((position.x - getVisionData()->crateData.offset) + 
							(getVisionData()->crateData.container_width - (getVisionData()->crateData.offset*2)) >= src.cols) {
				perror("<ERROR> container pixel location: x >= (camera)image width");
				position.x = (src.cols - 1) - (getVisionData()->crateData.container_width - (getVisionData()->crateData.offset*2));
			}
			if (position.y - getVisionData()->crateData.offset < 0) {
				perror("<ERROR> container pixel location: y < 0");
				position.y = getVisionData()->crateData.offset;
			} else if ((position.y - getVisionData()->crateData.offset) + 
							(getVisionData()->crateData.container_height - (getVisionData()->crateData.offset*2)) >= src.rows) {
				perror("<ERROR> container pixel location: y >= (camera)image height");
				position.x = (src.cols - 1) - (getVisionData()->crateData.container_width - (getVisionData()->crateData.offset*2));
			}
			
			getVisionData()->contentData.box[j + i*getVisionData()->crateData.col_nr] = src(Rect(	position.x - getVisionData()->crateData.offset, 
																																position.y - getVisionData()->crateData.offset, 
																																getVisionData()->crateData.container_width - (getVisionData()->crateData.offset*2), 
																																getVisionData()->crateData.container_height - (getVisionData()->crateData.offset*2)));	
																																
	/*		draw_box(src, 
					pre_perspective_pix.x - 3, 
					pre_perspective_pix.y - 3, 
					pre_perspective_pix.x + 3, 
					pre_perspective_pix.y + 3, 
					Scalar(0, 0, 255));*/
		}
	}
	
	
/*	
	for (int i = 0; i < getVisionData()->crateData.row_nr; i++) {
		for (int j = 0; j < getVisionData()->crateData.col_nr; j++) {
			cout << "bwidth: " << getVisionData()->contentData.width << ", bheight: " << getVisionData()->contentData.height << ", offset: " << getVisionData()->crateData.offset << endl;
			
			position.x = getVisionData()->contentData.width * j + getVisionData()->crateData.offset;
			position.y = getVisionData()->contentData.height * i + getVisionData()->crateData.offset;
			
			position = single_perspective_adjustment(position, false);
			
			
			getVisionData()->contentData.box[j + i*getVisionData()->crateData.col_nr] = src(Rect(position.x, 
																								position.y, 
																								getVisionData()->contentData.width-(getVisionData()->crateData.offset*2), 
																								getVisionData()->contentData.height-(getVisionData()->crateData.offset*2)));
		}
	}
*/
}

/* This function is used to get the crate from an image.
 *
 * To accomplish the desired result this function will cut an image out of the frame with a size large enough to contain a crate no matter the degree in which it is placed.
 * This new image will then be rotated and cut again to get the crate out.
 * 
 * @param Mat $frame
 * The main image the crate will be cut out.
 * 
 * @param Point2f $crate_pos
 * A point containing the x and y coordinates of the center of the crate.
 *
 * @param int $degree
 * The degree in which the crate is placed.
 *
 */
Mat Detect::get_roi(Mat frame, Point2f crate_pos, int degree, int width, int height) {
	Mat crate, rotate_dst, rot_mat, roi;	
	double temp_width, temp_height;
	temp_width = hypot(width, height);
	temp_height = hypot(width, height);
	
	
	int x1 = (int)crate_pos.x-0.5*temp_width;
	int y1 = (int)crate_pos.y-0.5*temp_height;
	int x2 = (int)temp_width;
	int y2 = (int)temp_height;
	
	if (x1 < 0) {x1 = 0; perror("<ERROR> in Detect::get_roi(): x1 < 0");}
	if (x1 > frame.cols) {x1 = frame.cols; perror("<ERROR> in Detect::get_roi(): x1 > frame.cols");}
	
	if (y1 < 0) {y1 = 0; perror("<ERROR> in Detect::get_roi(): y1 < 0");}
	if (y1 > frame.rows) {y1 = frame.rows; perror("<ERROR> in Detect::get_roi(): y1 > frame.rows");}
	
	if (x2 < 0) {x2 = 0; perror("<ERROR> in Detect::get_roi(): x2 < 0");}
	if (x2 > frame.cols) {x2 = frame.cols; perror("<ERROR> in Detect::get_roi(): x2 > frame.cols");}
	
	if (y2 < 0) {y2 = 0; perror("<ERROR> in Detect::get_roi(): y2 < 0");}
	if (y2 > frame.rows) {y2 = frame.rows; perror("<ERROR> in Detect::get_roi(): y2 > frame.rows");}
	roi = frame(Rect(x1, y1, x2, y2));

	Point center = Point((int)temp_width/2, (int)temp_height/2);

	rot_mat = getRotationMatrix2D(center, degree, 1.0);

	warpAffine(roi, rotate_dst, rot_mat, roi.size() );

	Mat final_roi;
	final_roi = rotate_dst(Rect(center.x-width/2, center.y-height/2, width, height));

	return final_roi;
}
